/* The register map is provided at
 * https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
 *
 * Mapping of the different gyro and accelero configurations:
 *
 * GYRO_CONFIG_[0,1,2,3] range = +- [250, 500,1000,2000] deg/s
 *                       sensi =    [131,65.5,32.8,16.4] bit/(deg/s)
 *
 * ACC_CONFIG_[0,1,2,3] range = +- [    2,   4,   8,  16] times the gravity (9.81 m/s^2)
 *                      sensi =    [16384,8192,4096,2048] bit/gravity
 */

 #ifndef MPU6050_LIGHT_H
 #define MPU6050_LIGHT_H
 
 #include "Arduino.h"
 #include "Wire.h"
 
 #define CHIP_MPU6050 0x01
 #define CHIP_LSM6DS3 0x02
 
 #define LSM6DS3_ADDR 0x6A // I2C从机地址
 #define LSM6DS3_WHO_AM_I 0x0F  // 设备ID寄存器地址
 #define LSM6DS3_WHO_AM_I_VALUE 0x6A // 正确ID值（用于验证连接）
 
 #define LSM6DS3_OUT_TEMP_REGISTER 0x20 // 温度数据起始地址
 #define LSM6DS3_OUT_G_REGISTER 0x22	 // 重力加速度数据起始地址
 #define LSM6DS3_OUT_XL_REGISTER 0x28 // 加速度计数据起始地址
 
 #define LSM6DS3_OUT_STATUS_MASK_TEMP  (1<<2) // 温度数据就绪标志位
 #define LSM6DS3_OUT_STATUS_MASK_G  (1<<1) // 陀螺仪数据就绪标志位
 #define LSM6DS3_OUT_STATUS_MASK_XL (1<<0) // 加速度计数据就绪标志位
 
 #define LSM6DS3_ACC_LSB_2G_PER       0.061 //定义加速度计在±2g量程下每个LSB（最小有效位）对应的物理值为0.061mg
 #define LSM6DS3_TEMP_LSB_PER_DEG 256.0 //定义温度传感器每摄氏度的LSB分辨率，℃对应256个LSB，即分辨率为 1/256 ℃/LSB
 #define LSM6DS3_TEMP_OFFSET_DEG 25 //校准温度传感器的零点偏移量
 
 #define MPU6050_ADDR 0x68 //定义 MPU6050 的I2C设备地址，AD0接地（低电平）时地址为 0x68（默认）
 #define MPU6050_SMPLRT_DIV_REGISTER 0x19 //用于设置传感器的采样频率
 #define MPU6050_CONFIG_REGISTER 0x1a //数字低通滤波器（DLPF）配置寄存器
 #define MPU6050_GYRO_CONFIG_REGISTER 0x1b //陀螺仪量程与精度配置寄存器
 #define MPU6050_ACCEL_CONFIG_REGISTER 0x1c //加速度计量程与精度配置寄存器
 #define MPU6050_PWR_MGMT_1_REGISTER 0x6b //电源管理寄存器1，控制传感器工作模式与时钟源
 
 #define MPU6050_GYRO_OUT_REGISTER 0x43 //陀螺仪数据输出寄存器
 #define MPU6050_ACCEL_OUT_REGISTER 0x3B //加速度计数据输出寄存器
 
 #define RAD_2_DEG 57.29578 //弧度转角度系数，用于将陀螺仪输出的角速度（弧度/秒）转换为度数/秒
 #define CALIB_OFFSET_NB_MES 500 //校准偏移量的采样次数，用于消除传感器零偏误差
 #define TEMP_LSB_2_DEGREE 340.0 //温度传感器每摄氏度对应的LSB（最小有效位）值 
 #define TEMP_LSB_OFFSET 12412.0 //温度传感器的零点偏移校准值，用于补偿硬件偏差
 
 #define DEFAULT_GYRO_COEFF 0.98 //姿态解算（如四元数或欧拉角计算）中具有关键作用
 
 class MPU6050
 {
 public:
     // INIT and BASIC FUNCTIONS
     MPU6050(TwoWire &w);
     /**
      * 重力加速度1 // range = +- 500 deg/s
      * 加速度计0  // range = +- 2 g
      */
     byte begin(int gyro_config_num = 1, int acc_config_num = 0);
 
     byte writeData(byte reg, byte data);
     byte readData(byte reg);
 
     void calcOffsets(bool is_calc_gyro = true, bool is_calc_acc = true);
     void calcGyroOffsets() { calcOffsets(true, false); }; // retro-compatibility with v1.0.0
     void calcAccOffsets() { calcOffsets(false, true); };  // retro-compatibility with v1.0.0
 
     void setAddress(uint8_t addr) { address = addr; };
     uint8_t getAddress() { return address; };
 
     // MPU CONFIG SETTER
     byte setGyroConfig(int config_num);
     byte setAccConfig(int config_num);
 
     void setGyroOffsets(float x, float y, float z);
     void setAccOffsets(float x, float y, float z);
 
     void setFilterGyroCoef(float gyro_coeff);
     void setFilterAccCoef(float acc_coeff);
 
     // MPU CONFIG GETTER
     float getGyroXoffset() { return gyroXoffset; };
     float getGyroYoffset() { return gyroYoffset; };
     float getGyroZoffset() { return gyroZoffset; };
 
     float getAccXoffset() { return accXoffset; };
     float getAccYoffset() { return accYoffset; };
     float getAccZoffset() { return accZoffset; };
 
     float getFilterGyroCoef() { return filterGyroCoef; };
     float getFilterAccCoef() { return 1.0 - filterGyroCoef; };
 
     // DATA GETTER
     float getTemp() { return temp; };
 
     float getAccX() { return accX; };
     float getAccY() { return accY; };
     float getAccZ() { return accZ; };
 
     float getGyroX() { return gyroX; };
     float getGyroY() { return gyroY; };
     float getGyroZ() { return gyroZ; };
 
     float getAccAngleX() { return angleAccX; };
     float getAccAngleY() { return angleAccY; };
 
     float getAngleX() { return angleX; };
     float getAngleY() { return angleY; };
     float getAngleZ() { return angleZ; };
 
     // INLOOP UPDATE
     void fetchData(); // user should better call 'update' that includes 'fetchData'
     void update();
 
     // UPSIDE DOWN MOUNTING
     bool upsideDownMounting = false;
 
 private:
     //指向I2C通信接口的指针，用于与MPU6050传感器进行数据传输
     TwoWire *wire;  
     //传感器I2C地址，默认值为0x68（AD0引脚接地时）。若AD0接高电平（VCC），地址变为0x69
     uint8_t address = MPU6050_ADDR; 
     //陀螺仪原始值（LSB）到角速度（度/秒）的转换系数
     float gyro_lsb_to_degsec;
     //加速度计原始值（LSB）到重力加速度（g）的转换系数
     float acc_lsb_to_g;
     //陀螺仪零偏校准值,分别是x，y，z方向
     float gyroXoffset, gyroYoffset, gyroZoffset;
     //加速度计零偏校准值，分别是x，y，z方向
     float accXoffset, accYoffset, accZoffset;
     //temp：温度值，accX/Y/Z：三轴加速度值，gyroX/Y/Z：三轴角速度值
     float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
     //angleAccX绕横轴（通常为左右方向轴）旋转的角度，angleAccY绕纵轴（通常为前后方向轴）旋转的角度
     float angleAccX, angleAccY;
     //X轴的旋转角，反映小车左右倾斜程度
     //Y轴的旋转角，反映小车前后俯仰状态。如加速或刹车时车身前倾或后仰 
     //Z轴的旋转角，表示小车绕垂直轴的转向角度，用于控制转弯方向
     float angleX, angleY, angleZ;
     //存储上一次姿态解算的时间戳
     unsigned long preInterval;
     //用于平衡陀螺仪积分角度与加速度计直接计算角度的比例
     float filterGyroCoef; 
 };
 
 #endif
 